#ifndef MY_PLANNER_H_
#define MY_PLANNER_H_

#include <ros/ros.h>
#include <nav_core/base_local_planner.h>



namespace my_planner
{
    class MyPlanner : public nav_core::BaseLocalPlanner
    {
    public:
        MyPlanner();
        ~MyPlanner();

        void initialize(std::string name, tf2_ros::Buffer *tf, costmap_2d::Costmap2DROS *costmap_ros);
        bool setPlan(const std::vector<geometry_msgs::PoseStamped> &plan);
        bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel);
        bool isGoalReached();

        //速度控制
        void speed_control(double dx, double dy, double *last_dx, double *last_dy, geometry_msgs::Twist &cmd_vel);

    private:
        std::vector<geometry_msgs::PoseStamped> global_plan_; // 全局路径
        int target_index_;                                    // 当前目标点在路径中的索引
        bool pose_adjusting_;                                 // 是否处于最终姿态调整阶段
        bool goal_reached_;                                   // 是否到达目标点

        //
        int final_flag = 0;
        int final_num = 0;

        double last_dx = 0;
        double last_dy = 0;
    };
} // namespace my_planner

#endif // MY_PLANNER_H_
